//========================================================================
// FILENAME : motorLeft.c
// CREATED : 07-03-2009
// AUTHOR : Jesper Aagaard Vuholm
// DESCR. : This file contains the implementation of our left motor control,
//			Using Timer1.
//
//------------------------------------------------------------------------
//
// REV. DATE/AUTHOR CHANGE DESCRIPTION
// 1.00 07-02-2009/Jesper prototype functions created.
// 1.01 11-02-2009/Jesper controlling functions finished and implemented.
// 1.02 27-04-2009/Jesper Motor hz is ~20khz, due to crystal change from 16Mhz. to 10Mhz.
// 1.02 05-05-2009/Jesper Joystick implemented to enable turn.
//========================================================================
#include "MotorDriver.h"

#define sbi(port,pin)    port |= 1<<pin    //set bit
#define cbi(port,pin)    port &= ~(1<<pin) //clear bit


//=============================================================
// METHOD : void motor_init(void)
// DESCR. : Enables the OC1A, OC1B, OC2A and OC2B pins(Port D pin 4-5-6-7).
//			Sets prescaler = 1 for both timer1 and timer2
//			Enable phase correct 8 bit pwm.
//=============================================================
void motor_init(void)
{
	///////////////////////
	//   TIMER 1 SETUP   //
	///////////////////////

	//Data direction PORTD
	DDRD |= (1<<PIND4)|(1<<PIND5);

	//Timer1 Control Register A (Outputs  /  wave mode & resolution) for more info, see: page 133.
	//PWM, phase correct 8-bit.
	//
	//	f_system clock / (prescaler * 2 * top_count) = f_pwm
	//
	//	10Mhz / (1 * 2 * 255 ) = 19,6Khz.
	TCCR1A |= (1<<COM1A1)|(1<<COM1A0)|(1<<COM1B1)|(1<<COM1B0)|(1<<WGM10);

	//Timer1 Control Register B (input compare / wave mode & resolution / prescaler)
	//Prescaler = 1.
	TCCR1B |= (1<<CS10);

	// Output compare register A for timer 1. Generated PWM is output on PORTD pin 5
	OCR1A = 0;
	// Output compare register B for timer 1. Generated PWM is output on PORTD pin 4
	OCR1B = 0;

	///////////////////////
	//   TIMER 2 SETUP   //
	///////////////////////

	//Data direction PORTD
	DDRD |= (1<<PIND6)|(1<<PIND7);

	//Timer2 Control Register (Outputs  /  wave mode & resolution) for more info, see: page 153.
	//PWM, phase correct 8-bit.
	//
	//	f_system clock / (prescaler * 2 * top_count) = f_pwm
	//
	//	10Mhz / (1 * 2 * 255 ) = 19,6Khz.
	TCCR2A |= (1<<COM2A1)|(1<<COM2A0)|(1<<COM2B1)|(1<<COM2B0)|(1<<WGM20);

	//Timer2 Control Register B (input compare / wave mode & resolution / prescaler)
	//Prescaler = 1.
	TCCR2B |= (1<<CS20);

	// Output compare register A for timer 2. Generated PWM is output on PORTD pin 7
	OCR2A = 0;
	// Output compare register B for timer 2. Generated PWM is output on PORTD pin 6
	OCR2B = 0;
}

//=============================================================
// METHOD : void motorLeft_set_speed(int speed)
// DESCR. : Sets the speed on left motor.
//=============================================================
void motorLeft_set_speed(int speed)
{
	if(speed > 0)
	{
		OCR1A = 0;
		OCR1B = speed;
	}
	else if(speed < 0)
	{
		OCR1B = 0;
		OCR1A = abs(speed);
	}
	else
	{
		OCR1B = 0;
		OCR1A = 0;
	}
}

//=============================================================
// METHOD : void motorRight_set_speed(int speed)
// DESCR. : Sets the speed on right motor.
//=============================================================
void motorRight_set_speed(int speed)
{
	if(speed > 0)
	{
		OCR2A = 0;
		OCR2B = speed;
	}
	else if(speed < 0)
	{
		OCR2B = 0;
		OCR2A = abs(speed);
	}
	else
	{
		OCR2B = 0;
		OCR2A = 0;
	}
}

//=============================================================
// METHOD : void runMotors(int speedLeft, int speedRight,signed int joystick)
// DESCR. : Adds the WHEEL_COMPENSATOR, this will even out the motors
// 		    due to different inner friction, so we will run straight.
//			Adds the SCALE_PWM factor to our speeds. We check that
//          we dont exceed MAX_PWM, then joystick is added. And a final
//          speed check is done to ensure that we max send 255 to the motors.
//=============================================================
void runMotors(int speedLeft, int speedRight,signed int joystick)
{
	speedLeft  *= WHEEL_COMPENSATOR;
	speedLeft  *= SCALE_PWM;
	speedRight *= SCALE_PWM;

	// This check ensures the ability to turn while at top speed.
	if (speedLeft <= -MAX_PWM) { speedLeft = -MAX_PWM; }
	else if (speedLeft >= MAX_PWM) { speedLeft = MAX_PWM; }
	if (speedRight <= -MAX_PWM) { speedRight = -MAX_PWM; }
	else if (speedRight >= MAX_PWM) { speedRight = MAX_PWM; }

	// Adds the joystick value to our speed.
	speedLeft += joystick;
	speedRight -= joystick;

	// This check ensures that the final speedvalue wont exceed 255(which would make it flip).
	if (speedLeft <= -MAX_PWM) { speedLeft = -MAX_PWM; }
	else if (speedLeft >= MAX_PWM) { speedLeft = MAX_PWM; }
	if (speedRight <= -MAX_PWM) { speedRight = -MAX_PWM; }
	else if (speedRight >= MAX_PWM) { speedRight = MAX_PWM; }

	motorLeft_set_speed(speedLeft);
	motorRight_set_speed(speedRight);
}

//=============================================================
// METHOD : void stopMotors(void)
// DESCR. : Sets the speed on left and right motor to zero.
//=============================================================
void stopMotors(void)
{
	motorLeft_stop();
	motorRight_stop();
}

//=============================================================
// METHOD : static void motorRight_stop(void)
// DESCR. : Sets the speed on right motor to zero.
//=============================================================
static void motorRight_stop(void)
{
	OCR2B = 0;
	OCR2A = 0;
}

//=============================================================
// METHOD : static void motorLeft_stop(void)
// DESCR. : Sets the speed on left motor to zero.
//=============================================================
static void motorLeft_stop(void)
{
	OCR1B = 0;
	OCR1A = 0;
}
